//
// Created by immo on 18-7-4.
//

#ifndef SOCKET_TEST_ARM_CONTROL_H_H
#define SOCKET_TEST_ARM_CONTROL_H_H

#endif //SOCKET_TEST_ARM_CONTROL_H_H

#include "socketlib.h"
#include <time.h>
class ArmControl{
public:
    ArmControl(){
        memset(&_status_now, 0, sizeof(_status_now));
        memset(&_status_set, 0, sizeof(_status_set));

        _sock_server=-1;
    }

    void ac_initial();

    void arm_init();

    void set_lr_angle(float l_angel[6],float r_angel[6]);
    void set_l_angle(float l_angel[6]);
    void set_r_angle(float r_angel[6]);

    void set_l_pose(float l_pose[6]);
    void set_r_pose(float r_pose[6]);
    void set_lr_pose(float l_pose[6],float r_pose[6]);

    void set_wheel(float v_go,float w_circle);
    void set_up_down(float up_down);

    void set_zero();
    void clear_warning();


    void ac_send(){
        int needSend=sizeof(_status_set);
        char buf[1024];
        memset(buf,0,1024);
        memcpy(buf,&_status_set,sizeof(_status_set)); //结构体转换成字符串

        while(1) {
            send(_sock_client, buf, needSend, 0);
            sleep(0.2);
            client_get_data();
            print_status_now();
            if ((_status_set.cmd1 == _status_now.cmd1) &&
                (_status_set.cmd2 == _status_now.cmd2) &&
                (_status_set.cmd3 == _status_now.cmd3) &&
                (_status_set.cmd4 == _status_now.cmd4)) {
                cout<<"server receive"<<endl;
                break;
            }

        }


    }

    int connect_to_robot(const char* SERVER_IP,const unsigned short SERVERPORT){
        _sock_client=socket_connect(SERVER_IP,SERVERPORT);
        return _sock_client;
    }

    void client_get_data(){
        char buf[1024];
        int len=recv(_sock_client,buf,1024,0);
        if(len<=0)return;
        if(buf[0]!='1'&&buf[0]!=1)return;
        memcpy(&_status_now,buf,sizeof(_status_now));
    }

    void print_status_now(){
        if(_status_now.cmd1==1)cout<<"int"<<endl;
        if(_status_now.cmd1=='1'){
            cout<<"char"<<endl;
        }
        cout << "cmd:[" << (int)_status_now.cmd1 << "," << (int)_status_now.cmd2 << "," << (int)_status_now.cmd3 << ","
             << (int)_status_now.cmd4 << "]" << endl;
        cout << "pose_l:[" << _status_now.l_x << "," << _status_now.l_y << "," << _status_now.l_z << ","
             << _status_now.l_a << ","
             << _status_now.l_b << "," << _status_now.l_c << "]" << endl;
        cout << "angle_l:[" << _status_now.l_angel1 << "," << _status_now.l_angel2 << "," << _status_now.l_angel3
             << "," << _status_now.l_angel4 << ","
             << _status_now.l_angel5 << "," << _status_now.l_angel6 << "," << _status_now.l_angel7<<"]" << endl;

        cout << "pose_r:[" << _status_now.r_x << "," << _status_now.r_y << "," << _status_now.r_z << ","
             << _status_now.r_a << ","
             << _status_now.r_b << "," << _status_now.r_c << "]" << endl;
        cout << "angle_r:[" << _status_now.r_angel1 << "," << _status_now.r_angel2 << "," << _status_now.r_angel3
             << "," << _status_now.r_angel4 << ","
             << _status_now.r_angel5 << "," << _status_now.r_angel6 <<","<< _status_now.r_angel7<< "]" << endl;
        cout << "wheel:[" << _status_now.left_wheel << "," << _status_now.right_wheel << "," << _status_now.up_down
             << "]" << endl;
        cout << endl;
    }
public:
    robot_info _status_now;
    robot_info _status_set;
private:
    int _sock_server;
    int _sock_client;
    int _client_fd;
};

void ArmControl::ac_initial() {

}

void ArmControl::set_lr_angle(float l_angel[6],float r_angel[6]){ // 需要获取机器人当前状态:目前第七轴返回原值，暂时不控制
    //if( ((uint)_status_now.cmd1) ==200 )
    _status_set=_status_now;
    _status_set.cmd1=1; // 标志，表示正常发送
    _status_set.cmd2=2;
    _status_set.cmd3=3;
    _status_set.cmd4=0;

    _status_set.l_angel1=l_angel[0];
    _status_set.l_angel2=l_angel[1];
    _status_set.l_angel3=l_angel[2];
    _status_set.l_angel4=l_angel[3];
    _status_set.l_angel5=l_angel[4];
    _status_set.l_angel6=l_angel[5];
    _status_set.l_angel7=0;

    _status_set.r_angel1=r_angel[0];
    _status_set.r_angel2=r_angel[1];
    _status_set.r_angel3=r_angel[2];
    _status_set.r_angel4=r_angel[3];
    _status_set.r_angel5=r_angel[4];
    _status_set.r_angel6=r_angel[5];
    _status_set.r_angel7=0;


    _status_set.left_wheel=0;  // 速度
    _status_set.right_wheel=0; // 角速度
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_l_angle(float l_angel[6]){ // 需要获取机器人当前状态:目前第七轴返回原值，暂时不控制
    //if( ((uint)_status_now.cmd1) ==200 )
    _status_set=_status_now;
    _status_set.cmd1=1; // 标志，表示正常发送
    _status_set.cmd2=2;
    _status_set.cmd3=1;
    _status_set.cmd4=0;

    _status_set.l_angel1=l_angel[0];
    _status_set.l_angel2=l_angel[1];
    _status_set.l_angel3=l_angel[2];
    _status_set.l_angel4=l_angel[3];
    _status_set.l_angel5=l_angel[4];
    _status_set.l_angel6=l_angel[5];
    _status_set.l_angel7=0;


    _status_set.left_wheel=0;  // 速度
    _status_set.right_wheel=0; // 角速度
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_r_angle(float r_angel[6]){ // 需要获取机器人当前状态:目前第七轴返回原值，暂时不控制
    //if( ((uint)_status_now.cmd1) ==200 )
    _status_set=_status_now;
    _status_set.cmd1=1; // 标志，表示正常发送
    _status_set.cmd2=2;
    _status_set.cmd3=2;
    _status_set.cmd4=0;


    _status_set.r_angel1=r_angel[0];
    _status_set.r_angel2=r_angel[1];
    _status_set.r_angel3=r_angel[2];
    _status_set.r_angel4=r_angel[3];
    _status_set.r_angel5=r_angel[4];
    _status_set.r_angel6=r_angel[5];
    _status_set.r_angel7=0;

    _status_set.left_wheel=0;  // 速度
    _status_set.right_wheel=0; // 角速度
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_zero(){
    _status_set=_status_now;
    _status_set.cmd1=1; // 标志，表示正常发送
    _status_set.cmd2=2;
    _status_set.cmd3=3;
    _status_set.cmd4=0;
    _status_set.l_angel1=0;
    _status_set.l_angel2=0;
    _status_set.l_angel3=0;
    _status_set.l_angel4=0;
    _status_set.l_angel5=0;
    _status_set.l_angel6=0;
    _status_set.l_angel7=0;

    _status_set.r_angel1=0;
    _status_set.r_angel2=0;
    _status_set.r_angel3=0;
    _status_set.r_angel4=0;
    _status_set.r_angel5=0;
    _status_set.r_angel6=0;
    _status_set.r_angel7=0;

    _status_set.left_wheel=0;
    _status_set.right_wheel=0;
    _status_set.up_down=0;

    ac_send();
}



void ArmControl::set_l_pose(float l_pose[6]){
    _status_set=_status_now;
    _status_set.cmd1=1;
    _status_set.cmd2=1;
    _status_set.cmd3=1;
    _status_set.cmd4=0;

    if(int(l_pose[0])!=0)_status_set.l_x-=l_pose[0];
    if(int(l_pose[1])!=0)_status_set.l_y-=l_pose[1];
    if(int(l_pose[2])!=0)_status_set.l_z-=l_pose[2];
    if(int(l_pose[3])!=0)_status_set.l_a-=l_pose[3];
    if(int(l_pose[4])!=0)_status_set.l_b-=l_pose[4];
    if(int(l_pose[5])!=0)_status_set.l_c-=l_pose[5];

    _status_set.left_wheel=0;
    _status_set.right_wheel=0;
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_r_pose(float r_pose[6]){
    _status_set=_status_now;
    _status_set.cmd1=1;
    _status_set.cmd2=1;
    _status_set.cmd3=2;
    _status_set.cmd4=0;

    if(int(_status_set.r_x)!=0)_status_set.r_x+=r_pose[0];
    if(int(_status_set.r_y)!=0)_status_set.r_y+=r_pose[1];
    if(int(_status_set.r_z)!=0)_status_set.r_z+=r_pose[2];
    if(int(_status_set.r_a)!=0)_status_set.r_a+=r_pose[3];
    if(int(_status_set.r_b)!=0)_status_set.r_b+=r_pose[4];
    if(int(_status_set.r_c)!=0)_status_set.r_c+=r_pose[5];

    _status_set.left_wheel=0;
    _status_set.right_wheel=0;
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_lr_pose(float l_pose[6],float r_pose[6]){
    _status_set=_status_now;
    _status_set.cmd1=1;
    _status_set.cmd2=1;
    _status_set.cmd3=3;
    _status_set.cmd4=0;


    if(int(_status_set.l_x)!=0)_status_set.l_x+=l_pose[0];
    if(int(_status_set.l_y)!=0)_status_set.l_y+=l_pose[1];
    if(int(_status_set.l_z)!=0)_status_set.l_z+=l_pose[2];
    if(int(_status_set.l_a)!=0)_status_set.l_a+=l_pose[3];
    if(int(_status_set.l_b)!=0)_status_set.l_b+=l_pose[4];
    if(int(_status_set.l_c)!=0)_status_set.l_c+=l_pose[5];

    if(int(_status_set.r_x)!=0)_status_set.r_x+=r_pose[0];
    if(int(_status_set.r_y)!=0)_status_set.r_y+=r_pose[1];
    if(int(_status_set.r_z)!=0)_status_set.r_z+=r_pose[2];
    if(int(_status_set.r_a)!=0)_status_set.r_a+=r_pose[3];
    if(int(_status_set.r_b)!=0)_status_set.r_b+=r_pose[4];
    if(int(_status_set.r_c)!=0)_status_set.r_c+=r_pose[5];

    _status_set.left_wheel=0;
    _status_set.right_wheel=0;
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_wheel(float v_go,float w_circle){
    _status_set=_status_now;

    _status_set.cmd1=1;
    _status_set.cmd2=0;
    _status_set.cmd3=0;
    _status_set.cmd4=0;

    _status_set.left_wheel=v_go;
    _status_set.right_wheel=w_circle;
    _status_set.up_down=0;

    ac_send();
}

void ArmControl::set_up_down(float up_down){
    _status_set=_status_now;

    _status_set.cmd1=1;
    _status_set.cmd2=0;
    _status_set.cmd3=0;
    _status_set.cmd4=0;

    _status_set.up_down=up_down;

    ac_send();


}

void ArmControl::clear_warning() {
    _status_set=_status_now;


    _status_set.cmd1=1;
    _status_set.cmd2=0;
    _status_set.cmd3=0;
    _status_set.cmd4=0;

    ac_send();


    _status_set.cmd1=1;
    _status_set.cmd2=0;
    _status_set.cmd3=0;
    _status_set.cmd4=1;

    ac_send();
}

void ArmControl::arm_init() {
    _status_set=_status_now;

    _status_set.cmd1=1;
    _status_set.cmd2=0;
    _status_set.cmd3=0;
    _status_set.cmd4=2;

    ac_send();
}